/*
 * @Author: liu Shihao
 * @Date: 2022-06-12 20:54:47
 * @LastEditors: liu Shihao
 * @LastEditTime: 2022-06-17 10:15:17
 * @FilePath: \code2\bearpi-hm_nano\applications\BearPi\BearPi-HM_Nano\sample\robot\src\communication_ros.c
 * @Description: 空闲中断
 * Copyright (c) 2022 by ${fzu} email: logic_fzu@outlook.com, All Rights Reserved.
 */
#include "communication_ros.h"
#include "hi_uart.h"
#include "hi_io.h"
// #include "cgp_net.h"
#define Reserve_Size  30
uint8_t  Reserve[Reserve_Size] = {0};
uint16_t remote_VX,remote_VY,remote_VZ,remote_VK;
#define UART_BUFF_SIZE 512     //接收缓冲大小
#define UART_TIMEOUT_MS 20      //接收超时时间
void rxdata_process(uint8_t *p);


//串口接收，无数据时阻塞，有数据时收完一帧或者收到最大值返回
uint16_t uart1_recv(uint8_t *buf,uint16_t len)
{
    uint16_t rxlen=0;
    uint16_t ret=0;
    while(len-rxlen>0){
        ret=hi_uart_read_timeout(HI_UART_IDX_1, buf+rxlen, len-rxlen,UART_TIMEOUT_MS);
        if(ret==0&&rxlen!=0){
            return rxlen;
        }
        else{
            rxlen+=ret;
        }
    }
    return rxlen;
}

void uart1_recvive_task(void)
{
    uint16_t rxlen;
    uint8_t uart_buff[UART_BUFF_SIZE] = {0};
    uint8_t *uart_buff_ptr = uart_buff;
    while (1)
    {
        //阻塞等待串口1数据
        printf("waiting\r\n");
        printf("KEY:%d  VX:%d  VY:%d  VZ:%d  \r\n",remote_VK ,remote_VX ,remote_VY ,remote_VZ);	
        rxlen=uart1_recv(uart_buff_ptr, UART_BUFF_SIZE);   
        rxdata_process(uart_buff_ptr);     
        printf("waitingoff:rxlen:%d\r\n",rxlen);
        printf("KEY:%d  VX:%d  VY:%d  VZ:%d  \r\n",remote_VK ,remote_VX ,remote_VY ,remote_VZ);
        //处理数据
        osDelay(1);
     }
}

 void ros_uart1_init(void)
{
    /*uart1 ros通信串口 */ 
   //初始化串口1
    hi_io_set_func(HI_IO_NAME_GPIO_0, HI_IO_FUNC_GPIO_0_UART1_TXD); /* uart1 tx */
    hi_io_set_func(HI_IO_NAME_GPIO_1, HI_IO_FUNC_GPIO_1_UART1_RXD); /* uart1 rx */
    hi_uart_attribute uart1_attr = {
        .baud_rate = 115200,
        .data_bits = 8,
        .stop_bits = 1,
        .parity = 0,
    };
    osDelay(5);
	uint32_t  ret = hi_uart_init(HI_UART_IDX_1, &uart1_attr, NULL);
    printf("init uart1 code = %d\n", ret);
    if (ret != HI_ERR_SUCCESS)
    {
        printf("Failed to init uart1 Err code = %d\n", ret);
        return;
    }
    //阻塞模式接收
    // 默认为阻塞模式
    //初始化串口驱动

    osThreadAttr_t attr;
    attr.name = "UART_Task";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 4096;
    attr.priority = 16;

    if (osThreadNew((osThreadFunc_t)uart1_recvive_task, NULL, &attr) == NULL)
    {
        printf("Falied to create UART1_Task!\n");
    }
}

/*
测量个标志位之间的长度     	M1X20Y40Z40*
*/
uint8_t Slice_Data(uint8_t *p)
{
	uint16_t len = 0;
	while(*(p+len+1) != 'M'&&*(p+len+1) != 'X'&&*(p+len+1) !='Y'&&*(p+len+1) != 'Z'&&*(p+len+1) !='*')
		len++;
	return len;
}
/*
 *空闲中断函数对数组进行处理
 */
void rxdata_process(uint8_t *p)
{
		uint16_t i,len=0;
    
    printf("receive_data_process\n");
	  if(*p !=0)
    {
    	remote_VK =0;   
        remote_VX=0;  	  
        remote_VY =0;	  
        remote_VZ =0;  
		
    		len = Slice_Data(p);						
			 p = p + len + 1;
			for(i=0;i<len;i++)
			remote_VK   += (*(p-i-1)-48)*pow(10,i);
		
			len = Slice_Data(p);
			p = p + len + 1;
			for(i=0;i<len;i++)
			remote_VX   += (*(p-i-1)-48)*pow(10,i);
			
			len = Slice_Data(p);
			p = p + len + 1;
			for(i=0;i<len;i++)
			remote_VY   += (*(p-i-1)-48)*pow(10,i);

			len = Slice_Data(p);
			p = p + len + 1;
			for(i=0;i<len;i++)
			remote_VZ  += (*(p-i-1)-48)*pow(10,i);
			printf("KEY:%d  VX:%d  VY:%d  VZ:%d  \r\n",remote_VK ,remote_VX ,remote_VY ,remote_VZ);	
    }
		memset(Reserve,0,Reserve_Size);
}



